%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  临时搭建的用于修补断线的脚本。跑的很慢，应该还可以大幅优化。
%
%- args:
%     fpspoint:  中间每次采样点的个数
%     look_area: x,y维度上查找的邻域范围
%     look_up:   z轴向上寻找的邻域范围
%     look_down: z轴向下寻找的邻域范围
%     look_times:  最大查找次数
%  脚本流程：
%     1、首先对于所有塔线点，进行一次最远点采样(fps)。
%     2、对每个点按参数寻找邻域，将邻域的点包括进来
%        P.S  这里对每个点寻找邻域很慢而且冗余，其实只要对边界点寻找就够了，但是我没写好
%     3、记录下包括进来的新点，如果大于采样点个数fpspoint则降采样，否则直接进行邻域点查找
%     4、重复2、3，直到到达最大查找次数look_times，或者不再有新点被找到。
%     在中间加入了如果找到的点太多会动态缩小邻域和找到点太少会动态扩大邻域的步骤。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fpspoint = 100;
look_area = 0.015;
look_up = 0.02;
look_down = 0.0005;
look_times = 30;

pc = sortrows(points,5,'descend');
ind = find(pc(:,5),1,'last');
linepoint = pc(1:ind,:);
save('linepoint.mat','linepoint');
fig = figure;hold on;
draw(linepoint(1:ind,:),'r');
px = sortrows(points,1);
lp = fps(linepoint,fpspoint);
old_points = points;
disp('Repair Line...');
for i = 1:size(lp,1)
    lx = lp(i,1)-look_area;
    rx = lp(i,1)+look_area;
    ly = lp(i,2)-look_area;
    ry = lp(i,2)+look_area;
    lz = lp(i,3)-look_down;
    rz = lp(i,3)+look_up;
    check(lx);
    check(rx);
    check(ly);
    check(ry);
    check(lz);
    check(rz);
    mskx = (px(:,1)>=lx) & (px(:,1)<rx);
    mskl = find(mskx,1,'first');
    mskr = find(mskx,1,'last');
    if (~isempty(mskr)) && (~isempty(mskl))
    	py = sortrows(px(mskl:mskr,:),2);
    	msky = (py(:,2)>=ly) & (py(:,2)<ry);
    	mskl = find(msky,1,'first');
    	mskr = find(msky,1,'last');
    	if (~isempty(mskr)) && (~isempty(mskl))
    		pz = sortrows(py(mskl:mskr,:),3);
    		mskz = (pz(:,3)>=lz) & (pz(:,3)<rz);
    		mskl = find(mskz,1,'first');
    		mskr = find(mskz,1,'last');
    		if (~isempty(mskr)) && (~isempty(mskl))
    			newlp = pz(mskl:mskr,:);
    			for j = 1:size(newlp,1)
    				points(newlp(j,4),5) = 1;
    			end
    		end
        end
    end
end
newmsk = find(points(:,5)-old_points(:,5));

look_times = look_times-1;
while look_times>0 && ~isempty(newmsk)
        if look_times==10
            look_down = 0;
        end
        newpoints = points(newmsk,:);
        disp(['find new line points: ', num2str(size(newpoints,1)),' || Remain look times:',num2str(look_times)]);
        if size(newpoints,1)>=fpspoint
            look_area = look_area-0.002;
            if look_area<0.008
                look_area = 0.008;
            end
        end
        if size(newpoints,1)<10
            look_area = look_area+0.005;
        end
        if size(newpoints,1)>fpspoint
            lp = fps(newpoints,fpspoint);
        else
            lp = newpoints;
        end
        old_points = points;
        for i = 1:size(lp,1)
            lx = lp(i,1)-look_area;
            rx = lp(i,1)+look_area;
            ly = lp(i,2)-look_area;
            ry = lp(i,2)+look_area;
            lz = lp(i,3)-look_down;
            rz = lp(i,3)+look_up;
            check(lx);
            check(rx);
            check(ly);
            check(ry);
            check(lz);
            check(rz);
            mskx = (px(:,1)>=lx) & (px(:,1)<rx);
            mskl = find(mskx,1,'first');
            mskr = find(mskx,1,'last');
            if (~isempty(mskr)) && (~isempty(mskl))
                py = sortrows(px(mskl:mskr,:),2);
                msky = (py(:,2)>=ly) & (py(:,2)<ry);
                mskl = find(msky,1,'first');
                mskr = find(msky,1,'last');
                if (~isempty(mskr)) && (~isempty(mskl))
                    pz = sortrows(py(mskl:mskr,:),3);
                    mskz = (pz(:,3)>=lz) & (pz(:,3)<rz);
                    mskl = find(mskz,1,'first');
                    mskr = find(mskz,1,'last');
                    if (~isempty(mskr)) && (~isempty(mskl))
                        newlp = pz(mskl:mskr,:);
                        for j = 1:size(newlp,1)
                            points(newlp(j,4),5) = 1;
                        end
                    end
                end
            end
        end
        newmsk = find(points(:,5)-old_points(:,5));
        look_times = look_times-1;
end
%show_results;


function result = check(input)
	result = input
    if input<0
		result = 0;
	end
	if input>1
		result = 1;
	end

end